An autonomous vehicle may need to plan a safe trajectory for itself based on its environment. The problem may be referred to as motion planning, and the vehicle doing the planning may be referred to as the host vehicle. For various reasons, motion planning may be a challenging problem for a host vehicle traveling on a highway. For example, the environment may be highly dynamic as the observed other vehicles may be moving fast. In order to avoid collision, the host vehicle may need to estimate the other vehicles' current position as well as predict their future intentions or trajectories. At the same time, the host vehicle may also be traveling at a high speed (e.g., 25-30 m/s) itself. Therefore, motion planning may require taking into account the complex environment as well as the goal of the host vehicle (e.g., the host vehicle may need to take the next highway exit).
Further, in addition to collision avoidance, there may be other desired objectives in the motion planning problem. For example, it may be preferable to have a planned trajectory that can be effectuated with a smooth control signal in order to improve efficiency.